#include "scara.h"
#include "omni.h"
#include <iostream>
#include <cmath>
#include <cstring>
#include <cstdio>   /* Standard input/output definitions */
#include <cstdlib>
// #include "cv.h"
// #include "highgui.h"
// #include "unistd.h"

using namespace std;

#define STEP 0.5 // em cm
#define MM 100 // em mm

//scara myScara(ARDUINO_SCARA); //cria um objeto scara e linka com o arduinoScara
omni myOmni(ARDUINO_OMNI);

int main(void)
{
  float X = 17.0,Y = 0.0,Z = 0.0,orientation = 0.0; // POSICAO INICIAL DO SCARA
  int leitura = 0;
  char cmd[10];
  int k,comando = -1;
  FILE* fp;
  
  
  if( (fp=fopen("socketCmd","r")) == NULL )
  {
    printf("Cannot open file. socketCmd\n");
    exit(1);
  }
  
  //cout << "Esperando Start..." << endl;
  
  // While( arduino nao der start ) { esperar }
  //myScara.esperaStart();

  //cout << "START DADO!" << endl;
  //myScara.flush(); // apaga as baboseiras da serial...
  
  //myScara.scaraGoTo(X,Y,orientation);

  
    while( true )
    {
	    cmd[0] = '\0';
	    k = fscanf(fp, "%s", cmd);
	    
	    if( k != EOF )	
	    {
		    cout << "lido isso do arquivo:" << cmd << endl;
		    
		    if( strcmp(cmd,"frente") == 0 )
		      comando = FRENTE;
		    else
		      if( strcmp(cmd,"tras") == 0 )
			comando = TRAS;
		      else
			if( strcmp(cmd,"direita") == 0 )
			  comando = DIR;
			else
			  if( strcmp(cmd,"esquerda") == 0 )
			    comando = ESQ;
			  else
			    if( strcmp(cmd,"giraH") == 0 )
			      comando = CW;
			    else
			      if( strcmp(cmd,"giraAH") == 0 )
				comando = CCW;
			      else
				continue;
		    
			    
		if( (comando == FRENTE)||(comando == TRAS)||(comando == DIR)||(comando == ESQ) )	      
		  myOmni.go(comando,MM);
		else
		  if( (comando == CCW)||(comando == CW))
		    myOmni.turn(comando,90);
	   }

    }   
  
return 0;
}

